
import robo3pi as a
import time
import math
import serial
global ser

RIGHT_WHEEL_OFFSET = 0
LEFT_WHEEL_OFFSET = 0
ROTATION_TIME_360 = 2.4
EPSILON = 0.03
WIDTH = 20
HIGHT = 20
    
# Distance in meters, Velocity in M/S (Meters per second)
def moveInLine(id, distance, velocity):
    error = -1
    
    if (velocity > 1 or velocity == 0):
        print ("Velocity is too big")
        return error
    
    movingTime = (distance/velocity)
    velocity = velocity * 255 # Transform velocity to robot values
    
    print ("Velocy is " + str(velocity))
    print ("Movement time is: " + str(movingTime))
    
    if (abs(movingTime) == 0):
        print ("Time has incorrect value. System ABORTED :)")
        return error
    
    a.move(id, velocity + LEFT_WHEEL_OFFSET, velocity + RIGHT_WHEEL_OFFSET) #TODO: Try and avoid offsets
    time.sleep(movingTime)
    a.stop(id)
    time.sleep(EPSILON)
    
def stop(id, seconds):
    time.sleep(seconds)
    
    
def rotateByAngle(id, angle):
    #TODO: using default velocity value, consider using parameter
    rotationTime = abs ( (angle / 360.0) * ROTATION_TIME_360 )
    
    print ("Rotation time is: " + str(rotationTime))
    
    if angle > 0:
        motorSpeed = [-30, 30]
    else:
        motorSpeed = [30, -30]
    
    a.move(id, motorSpeed[0], motorSpeed[1])   
      
    time.sleep(rotationTime) #TODO: our own sleep
    print ("finished sleep")
    a.stop(id) 
    print ("finished stop")
    time.sleep(EPSILON)
    return 1
 
def moveInRectangle(id, edgeLength, seconds): 
    
    for i in range (0, 4):
        moveInLine(id, edgeLength, seconds/4)
        print ("finished move")        
        rotateByAngle(id, 90)
         
    return 1

def moveInPolygon(id, numOfEdges, edgeLength, seconds):
    
    if (numOfEdges == 3):
        angle = 60
    elif (numOfEdges == 4):
        angle = 90
    elif (numOfEdges == 5):
        angle = 108
    elif (numOfEdges == 6):
        angle = 120
    elif (numOfEdges == 7):
        angle = 128.5
    elif (numOfEdges == 8):
        angle = 135
        
    angle = 180 - angle
        
    for i in range (0, numOfEdges):
        
        moveInLine(id, edgeLength, seconds/numOfEdges)
        print ("finished move")        
        time.sleep(EPSILON)
        rotateByAngle(id, angle)
        
    
    return 1



def moveInLineOld(id, distance, seconds):
    error = -1
    velocity = (distance/seconds) * 255
    
    print ("Velocity is: " + str(velocity))
    
    if (abs(velocity) > 255 or abs(velocity) < 20):
        print ("Velocity has incorrect value. System ABORTED :)")
        return error
    
    a.move(id, velocity + LEFT_WHEEL_OFFSET, velocity + RIGHT_WHEEL_OFFSET) #TODO: Try and avoid offsets
    time.sleep(seconds)
    a.stop(id)
    time.sleep(EPSILON)
    
def normalization ( x , y , width):
    nx = (x/width)*(2)-1
    ny = (y/width)*(2)-1
    point = [ nx , ny ]
    return point
    

    
    